package com.hitqz.robot.biz.flow.component.agv;

import cn.hutool.extra.spring.SpringUtil;
import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.google.common.cache.Cache;
import com.hitqz.robot.api.business.enums.CmdNavStatusEnum;
import com.hitqz.robot.api.business.enums.NavEnum;
import com.hitqz.robot.api.business.enums.RobotOtherCommandEnum;
import com.hitqz.robot.api.business.model.dto.FlowContextDto;
import com.hitqz.robot.api.business.model.dto.FlowPatrolPreAndEndTicketsDto;
import com.hitqz.robot.api.common.dto.robot.RobotGoTargetPosDto;
import com.hitqz.robot.api.common.entity.Robot;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.api.common.entity.TicketsTask;
import com.hitqz.robot.biz.business.dispatch.DispatchService;
import com.hitqz.robot.biz.config.RobotProperties;
import com.hitqz.robot.biz.flow.config.FlowInterruptException;
import com.hitqz.robot.biz.flow.util.LiteFlowUtil;
import com.hitqz.robot.biz.schedule.CheckNavStatusSchedule;
import com.hitqz.robot.biz.service.RobotMapPosService;
import com.hitqz.robot.biz.service.RobotService;
import com.hitqz.robot.common.core.util.R;
import com.hitqz.robot.driver.model.IRobotPosDto;
import com.hitqz.robot.driver.model.RobotIOStatusDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.yomahub.liteflow.annotation.LiteflowComponent;
import com.yomahub.liteflow.core.NodeComponent;
import lombok.AllArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.util.StringUtils;

import java.util.*;
import java.util.concurrent.TimeUnit;

/**
 * @author xupkun
 * @date 2024/12/3
 */

@Slf4j
@LiteflowComponent(value = "GoTargetAndWaitForLifter", name = "[轮对/轮轴]前往取轮对")
@AllArgsConstructor
public class GoTargetAndWaitForLifter extends NodeComponent {

    private final RobotDriverService robotDriverService;

    private final RobotMapPosService robotMapPosService;

    private final RobotService robotService;

    private final DispatchService dispatchService;

    private static final int CMD_INVALID = -1;

    private static List<String> canLifterPos = Arrays.asList("LM39", "LM43", "LM2", "LM40", "LM42", "LM4");

    private final RobotProperties robotProperties;

    @Override
    public void process() throws Exception {
        LiteFlowUtil.checkInterruptFlag();

        IRobotPosDto iRobotPosDto = robotDriverService.getWebPos();
        R currentResult = robotDriverService.executeOtherCommand(RobotOtherCommandEnum.JACKING_STATUS, null);
        int currentJackStatus = (int) currentResult.getData();
        if (iRobotPosDto.getFull() && currentJackStatus == 1) {
            log.info("AGV带载，且顶升上升状态，不去起点，直接去目标点");
            return;
        }
        FlowContextDto dto = this.getRequestData();
        TicketsTask task = dto.getTicketsTask();
        log.info("任务数据 {}", task.getParseOrderParams());
        Robot robot = robotService.getDefaultRobot();
        FlowPatrolPreAndEndTicketsDto ticketsDto = JSONObject.parseObject(dto.getTicketsTask()
                .getParseOrderParams(), FlowPatrolPreAndEndTicketsDto.class);
        String sourceName = Optional.ofNullable(ticketsDto.getModalData().get("sourcePosName"))
                .orElse(ticketsDto.getModalData().get("sourcePosCode")).toString();
        String targetPosName = Optional.ofNullable(ticketsDto.getModalData().get("targetPosName"))
                .orElse(ticketsDto.getModalData().get("targetPosCode")).toString();

        RobotMapPos robotMapPos = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                .eq(RobotMapPos::getName, sourceName)
                .eq(RobotMapPos::getMapCode, robot.getMapCode()));
        if (robotMapPos == null) {
            robotMapPos = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                    .eq(RobotMapPos::getCode, sourceName)
                    .eq(RobotMapPos::getMapCode, robot.getMapCode()));
        }
        log.info("前往终端目标点{}", robotMapPos.getCode());
        boolean isSuccess = dispatchService.go2TargetPosition(robotMapPos, false);
        while (true) {
            try {
                iRobotPosDto = robotDriverService.getWebPos();
                LiteFlowUtil.checkInterruptFlag();
                if (iRobotPosDto == null || iRobotPosDto.getTaskStatus() == null) {
                    continue;
                }
                if (iRobotPosDto.getTaskStatus() != null) {
                    int taskStatus = iRobotPosDto.getTaskStatus();
                    boolean isSuspendedOrFailed = taskStatus == NavEnum.SUSPENDED.getCode() || taskStatus == NavEnum.FAILED.getCode() ||
                            taskStatus == NavEnum.CANCEL.getCode();
                    String currentPosCode = StringUtils.hasText(iRobotPosDto.getLastStation()) ? iRobotPosDto.getLastStation()
                            : iRobotPosDto.getCurrentStation();
                    log.info("导航的状态: {} --- {} --- currentPosCode: {}",
                            iRobotPosDto.getTaskStatus(), isSuspendedOrFailed, currentPosCode);

                    if (isSuspendedOrFailed && canLifterPos.contains(currentPosCode)) {
                        iRobotPosDto = robotDriverService.getWebPos();
                        if (iRobotPosDto == null || iRobotPosDto.getTaskStatus() == null) {
                            continue;
                        }
                        //log.info("获取到的状态为:" + JSONObject.toJSONString(iRobotPosDto));
                        RobotIOStatusDto robotIOStatusDto = iRobotPosDto.getDI().stream().filter(
                                t -> t.getId() == 0).findFirst().get();
                        for (int i = 0; i < 20; i++) {
                            if (robotIOStatusDto.isStatus()) {
                                log.info("循环{}次数可以举起来:", i, robotIOStatusDto.isStatus());
                                break;
                            } else {
                                iRobotPosDto = robotDriverService.getWebPos();
                                robotIOStatusDto = iRobotPosDto.getDI().stream().filter(
                                        t -> t.getId() == 0).findFirst().get();
                                log.info("循环重新获取：" + robotIOStatusDto.isStatus());
                                Thread.sleep(300);
                            }
                        }
                        log.info("上面的传感器数据: {}---{}", robotIOStatusDto.getId(), robotIOStatusDto.isStatus());
                        if (!robotIOStatusDto.isStatus()) {
                            boolean goStation = !robotMapPos.getCode().equals("LM4") || !iRobotPosDto.getLastStation().equals("LM5");
                            if (robotMapPos.getCode().equals("LM2") && iRobotPosDto.getLastStation().equals("LM3")) {
                                goStation = false;
                            }
                            // LM2 LM3
                            log.info("lifter状态为，前往{}--- {}----{}", robotMapPos.getCode(), iRobotPosDto.getLastStation(), goStation);
                            if (goStation) {
                                log.info("继续前往：" + robotMapPos.getId().intValue());
                                dispatchService.go2TargetPosition(robotMapPos, false);
                            } else {
                                boolean lifter = robotMapPos.getCode().equals("LM4") && iRobotPosDto.getLastStation().equals("LM5")
                                        && (!iRobotPosDto.getCurrentStation().equals("LM5"));
                                if (robotMapPos.getCode().equals("LM2") && iRobotPosDto.getLastStation().equals("LM3")
                                        && (!iRobotPosDto.getCurrentStation().equals("LM3"))) {
                                    lifter = true;
                                }
                                if (lifter) {
                                    String target = "LM3";
                                    if (robotMapPos.getCode().equals("LM4")) {
                                        target = "LM5";
                                    }
                                    log.info("超过之后，往回退---------------{}", target);
                                    RobotMapPos back = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                                            .eq(RobotMapPos::getCode, target)
                                            .eq(RobotMapPos::getMapCode, robot.getMapCode()));
                                    dispatchService.go2TargetPosition(back, false);
                                }
                            }
                        } else {
                            if (robotIOStatusDto.isStatus()) {
                                boolean lifter = true;
                                if (lifter) {
                                    String shelf = "shelf/s0002.shelf";
                                    log.info("可以举起来" + shelf);
                                    if (robotProperties.isDebug()) {
                                        String auto = ticketsDto.getModalData().get("auto") == null ? "0" :
                                                String.valueOf(ticketsDto.getModalData().get("auto"));
                                        if (auto.equals("0")) {
                                            log.info("半自动任务:跳过");
                                            return;
                                        }
                                    }

                                    robotDriverService.executeOtherCommand(RobotOtherCommandEnum.JACKING_UP, shelf);
                                    TimeUnit.SECONDS.sleep(8);
                                    R result = robotDriverService.executeOtherCommand(RobotOtherCommandEnum.JACKING_STATUS, null);
                                    int jackStatus = (int) result.getData();
                                    boolean hasStop = false;
                                    while (0x01 != jackStatus) {
                                        result = robotDriverService.executeOtherCommand(RobotOtherCommandEnum.JACKING_STATUS, null);
                                        jackStatus = (int) result.getData();
                                        iRobotPosDto = robotDriverService.getWebPos();
                                        robotIOStatusDto = iRobotPosDto.getDI().stream().filter(
                                                t -> t.getId() == 0).findFirst().get();
                                        if (!robotIOStatusDto.isStatus()) {
                                            log.info("顶升过程检测不到信号:下降");
                                            robotDriverService.navCmd(CmdNavStatusEnum.NAV_CANCEL.getCode());
                                            TimeUnit.SECONDS.sleep(2);
                                            // 顶升下降
                                            robotDriverService.executeOtherCommand(RobotOtherCommandEnum.JACKING_DOWN, null);
                                            robotDriverService.executeOtherCommand(RobotOtherCommandEnum.JACKING_DOWN, null);
                                            TimeUnit.SECONDS.sleep(8);
                                            // 获取状态
                                            result = robotDriverService.executeOtherCommand(RobotOtherCommandEnum.JACKING_STATUS, null);
                                            CheckNavStatusSchedule.navStatus = 1;
                                            int i = 0;
                                            while (true) {
                                                jackStatus = (int) result.getData();
                                                if (0x03 == jackStatus) {
                                                    log.info("顶升过程检测不到信号:下降完成");
                                                    hasStop = true;
                                                    break;
                                                }
                                                if (i == 10) {
                                                    log.info("10S顶升没下降到位，发送继续下降");
                                                    robotDriverService.executeOtherCommand(RobotOtherCommandEnum.JACKING_DOWN, null);
                                                    i = 0;

                                                }
                                                LiteFlowUtil.checkInterruptFlag();
                                                result = robotDriverService.executeOtherCommand(RobotOtherCommandEnum.JACKING_STATUS, null);
                                                TimeUnit.SECONDS.sleep(1);
                                                i++;
                                            }
                                            CheckNavStatusSchedule.navStatus = 0;
                                            if (hasStop) {
                                                break;
                                            }
                                        }
                                        LiteFlowUtil.checkInterruptFlag();
                                    }
                                    if (hasStop) {
                                        log.info("顶升过程发生异常重新来");
                                        String target = "LM3";
                                        if (robotMapPos.getCode().equals("LM4")) {
                                            target = "LM5";
                                        }
                                        log.info("超过之后，往回退---------------{}", target);
                                        RobotMapPos back = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                                                .eq(RobotMapPos::getCode, target)
                                                .eq(RobotMapPos::getMapCode, robot.getMapCode()));
                                        dispatchService.go2TargetPosition(back, false);
                                        continue;
                                    }
                                    break;
                                } else {
                                    log.info("超过之后，往回退------");
                                    String target = "LM3";
                                    if (robotMapPos.getCode().equals("LM4")) {
                                        target = "LM5";
                                    }
                                    RobotMapPos back = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                                            .eq(RobotMapPos::getCode, target)
                                            .eq(RobotMapPos::getMapCode, robot.getMapCode()));
                                    dispatchService.go2TargetPosition(back, false);
                                }
                            }
                        }
                    } else if (iRobotPosDto.getTaskStatus() == 4) {
                        if (iRobotPosDto.getCurrentStation().equals("LM3") || iRobotPosDto.getCurrentStation().equals("LM5")) {
                            log.info("回退之后继续前进目标点");
                            dispatchService.go2TargetPosition(robotMapPos, false);
                        } else if (iRobotPosDto.getCurrentStation().equals("LM2") || iRobotPosDto.getCurrentStation().equals("LM4")) {
                            break;
                        }
                    }
                    TimeUnit.SECONDS.sleep(1);
                }
            } catch (Exception e) {
                log.info("解析异常:" + e.getMessage());
                if (e instanceof FlowInterruptException) {
                    log.info("流程被中断");
                    break;
                }
            }
        }
    }
}
